﻿//------------------------------------------------------------------------------
// <copyright file="MainWindow.xaml.cs" company="Microsoft">
//     Copyright (c) Microsoft Corporation.  All rights reserved.
// </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Samples.Kinect.BodyBasics
{
    using System;
    using System.Collections.Generic;
    using System.ComponentModel;
    using System.Diagnostics;
    using System.Globalization;
    using System.IO;
    using System.Windows;
    using System.Windows.Media;
    using System.Windows.Media.Imaging;
    using Microsoft.Kinect;
    using MathNet.Numerics.LinearAlgebra.Single;
    using System.Numerics;

    /// <summary>
    /// Interaction logic for MainWindow
    /// </summary>
    public partial class MainWindow : System.Windows.Window, INotifyPropertyChanged
    {
        #region MatlabInit
        //Calculator.InversKinematics inversKinematics;
        Calculator.RoboArmInverse roboArm;
        private void Init_inversKinematics()
        {
            roboArm = new Calculator.RoboArmInverse();
        }
        #endregion

        #region Communicate
        Controls.TCPClient tcpClient = null;
        Controls.SerialPort serialPort = null;

        private void Init_Communicate()
        {
            tcpClient = ctl_tcpClient;
            serialPort = ctl_serialPort;
        }

        private void sendString(string str)
        {
            Controls.Package package = Controls.Package.AddPackage(str);
            tcpClient.AddPackage(package);
            serialPort.AddPackage(package);
        }

        #endregion

        #region Servos

        List<Controls.Servo> servos = null;

        int actionTime = 1000;

        Controls.Servo servo1 = null;
        Controls.Servo servo2 = null;
        Controls.Servo servo3 = null;
        Controls.Servo servo4 = null;
        Controls.Servo servo5 = null;
        Controls.Servo servo6 = null;
        Controls.Servo servo7 = null;
        private void Init_servos()
        {
            servos = new List<Controls.Servo>();
            servo1 = svosld_servo1.Servo;
            servos.Add(servo1);
            servo2 = svosld_servo2.Servo;
            servos.Add(servo2);
            servo3 = svosld_servo3.Servo;
            servos.Add(servo3);
            servo4 = svosld_servo4.Servo;
            servos.Add(servo4);
            servo5 = svosld_servo5.Servo;
            servos.Add(servo5);
            servo6 = svosld_servo6.Servo;
            servos.Add(servo6);
            servo7 = svosld_servo7.Servo;
            servos.Add(servo7);

            svosld_servo1.ServoPWMChangedEvent += Svosld_servo_ServoPWMChangedEvent;
            svosld_servo2.ServoPWMChangedEvent += Svosld_servo_ServoPWMChangedEvent;
            svosld_servo3.ServoPWMChangedEvent += Svosld_servo_ServoPWMChangedEvent;
            svosld_servo4.ServoPWMChangedEvent += Svosld_servo_ServoPWMChangedEvent;
            svosld_servo5.ServoPWMChangedEvent += Svosld_servo_ServoPWMChangedEvent;
            svosld_servo6.ServoPWMChangedEvent += Svosld_servo_ServoPWMChangedEvent;
            svosld_servo7.ServoPWMChangedEvent += Svosld_servo_ServoPWMChangedEvent;

            servo1.MaxPwmRange = 2500;
            servo1.MinPwmRange = 1500;
            servo1.InitPwm = 1500;
            svosld_servo1.UpdateLayout();
            //servo1.Diff_pwm = 0;

            servo2.MaxPwmRange = 2200;
            servo2.MinPwmRange = 1000;
            servo2.InitPwm = 1000;
            //servo2.Diff_pwm = 500;

            servo3.MaxPwmRange = 2200;
            servo3.MinPwmRange = 1000;
            servo3.InitPwm = 2100;
            //servo3.Diff_pwm = 500;

            servo4.MaxPwmRange = 2500;
            servo4.MinPwmRange = 500;
            servo4.InitPwm = 1500;
            //servo4.Diff_pwm = 0;

            servo5.MaxPwmRange = 2000;
            servo5.MinPwmRange = 800;
            servo5.InitPwm = 2200;

            servo6.MaxPwmRange = 1800;
            servo6.MinPwmRange = 700;
            servo6.InitPwm = 1500;

            servo7.MaxPwmRange = 1850;
            servo7.MinPwmRange = 1150;
            servo7.InitPwm = 1500;

            

        }

        private void setDegree2deltaPwm(double degree,int number)
        {
            int deltaPwm = 0;
            switch (number)
            {
                case 1:deltaPwm = (int)(degree * 8.3);
                    servo1.Diff_pwm = deltaPwm;
                    svosld_servo1.sld_PWM.Value = servo1.Abs_pwm;
                    break;
                case 2:deltaPwm = (int)(degree * 4.6);
                    servo2.Diff_pwm = deltaPwm;
                    svosld_servo2.sld_PWM.Value = servo2.Abs_pwm;
                    break;
                case 3:deltaPwm = (int)(degree * 3.5);
                    servo3.Diff_pwm = deltaPwm;
                    svosld_servo3.sld_PWM.Value = servo3.Abs_pwm;
                    break;
                case 4:deltaPwm = (int)(degree * 7.4);
                    servo4.Diff_pwm = deltaPwm;
                    svosld_servo4.sld_PWM.Value = servo4.Abs_pwm;
                    break;
                case 5: deltaPwm = (int)(degree * -3.7);
                    servo5.Diff_pwm = deltaPwm;
                    svosld_servo5.sld_PWM.Value = servo5.Abs_pwm;
                    break;
                case 6: deltaPwm = (int)(degree * 7.4);
                    servo6.Diff_pwm = deltaPwm;
                    svosld_servo6.sld_PWM.Value = servo6.Abs_pwm;
                    break;
                case 7:deltaPwm = (int)(degree * 11);
                    servo7.Diff_pwm = deltaPwm;
                    svosld_servo7.sld_PWM.Value = servo7.Abs_pwm;
                    break;
                default:deltaPwm = (int)(degree * (2000 / 180));
                    //servo1.Diff_pwm = deltaPwm;
                    break;
            }
            //return deltaPwm;
        }

        private void btn_servoControl_Click(object sender, RoutedEventArgs e)
        {
            try
            {
                //直接得到角度
                int deg1 = int.Parse(txb_degSer1.Text);
                int deg2 = int.Parse(txb_degSer2.Text);
                int deg3 = int.Parse(txb_degSer3.Text);
                int deg4 = int.Parse(txb_degSer4.Text);
                int deg5 = int.Parse(txb_degSer5.Text);
                int deg6 = int.Parse(txb_degSer6.Text);

                //转换为pwm并输出到servo，由servo触发事件发送
                setDegree2deltaPwm(deg1, 1);
                setDegree2deltaPwm(deg2, 2);
                setDegree2deltaPwm(deg3, 3);
                setDegree2deltaPwm(deg4, 4);
                setDegree2deltaPwm(deg5, 5);
                setDegree2deltaPwm(deg1, 6);

                Console.WriteLine("degree changed");
            }
            catch(Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }

        private void txb_servoTime_LostFocus(object sender, RoutedEventArgs e)
        {
            try
            {
                //int time = int.Parse(txb_servoTime.Text);
                actionTime = int.Parse(txb_servoTime.Text);

            }
            catch(Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }

        private void txb_servoTime_KeyUp(object sender, System.Windows.Input.KeyEventArgs e)
        {
            if (e.Key == System.Windows.Input.Key.Enter)
            {
                txb_servoTime_LostFocus(null, null);
            }
        }

        private void Svosld_servo_ServoPWMChangedEvent(Controls.Servo servo)
        {
            string str = "";
            foreach (Controls.Servo s in servos)
            {
                str += s.GetEncoderString();
            }

            //单个发送
            //str += servo.GetEncoderString();


            str += "T" + actionTime.ToString();
            sendString(str);
            //Console.WriteLine(str);

        }

        //设置6个舵机的角度
        private void setServosPWM(int[] pwmArray)
        {
            svosld_servo1.sld_PWM.Value = pwmArray[0];
            svosld_servo2.sld_PWM.Value = pwmArray[1];
            svosld_servo3.sld_PWM.Value = pwmArray[2];
            svosld_servo4.sld_PWM.Value = pwmArray[3];
            svosld_servo5.sld_PWM.Value = pwmArray[4];
            svosld_servo6.sld_PWM.Value = pwmArray[5];
            //svosld_servo7.txb_absPWM.Text = pwmArray[0].ToString();
        }
        private void setHand(int pwm)
        {
            //svosld_servo7.txb_absPWM.Text = pwm.ToString();
            svosld_servo7.sld_PWM.Value = pwm;
        }

        #endregion

        #region Draw
        /// <summary>
        /// Radius of drawn hand circles
        /// </summary>
        private const double HandSize = 30;

        /// <summary>
        /// Thickness of drawn joint lines
        /// </summary>
        private const double JointThickness = 3;

        /// <summary>
        /// Thickness of clip edge rectangles
        /// </summary>
        private const double ClipBoundsThickness = 10;

        /// <summary>
        /// Constant for clamping Z values of camera space points from being negative
        /// </summary>
        private const float InferredZPositionClamp = 0.1f;

        /// <summary>
        /// Brush used for drawing hands that are currently tracked as closed
        /// </summary>
        private readonly Brush handClosedBrush = new SolidColorBrush(Color.FromArgb(128, 255, 0, 0));

        /// <summary>
        /// Brush used for drawing hands that are currently tracked as opened
        /// </summary>
        private readonly Brush handOpenBrush = new SolidColorBrush(Color.FromArgb(128, 0, 255, 0));

        /// <summary>
        /// Brush used for drawing hands that are currently tracked as in lasso (pointer) position
        /// </summary>
        private readonly Brush handLassoBrush = new SolidColorBrush(Color.FromArgb(128, 0, 0, 255));

        /// <summary>
        /// Brush used for drawing joints that are currently tracked
        /// </summary>
        private readonly Brush trackedJointBrush = new SolidColorBrush(Color.FromArgb(255, 68, 192, 68));

        /// <summary>
        /// Brush used for drawing joints that are currently inferred
        /// </summary>        
        private readonly Brush inferredJointBrush = Brushes.Yellow;

        /// <summary>
        /// Pen used for drawing bones that are currently inferred
        /// </summary>        
        private readonly Pen inferredBonePen = new Pen(Brushes.Gray, 1);

        /// <summary>
        /// Drawing group for body rendering output
        /// </summary>
        private DrawingGroup drawingGroup;

        /// <summary>
        /// Drawing image that we will display
        /// </summary>
        private DrawingImage imageSource;
        #endregion

        #region Kinect


        /// <summary>
        /// Active Kinect sensor
        /// </summary>
        private KinectSensor kinectSensor = null;

        /// <summary>
        /// Coordinate mapper to map one type of point to another
        /// </summary>
        private CoordinateMapper coordinateMapper = null;

        /// <summary>
        /// Reader for body frames
        /// </summary>
        private BodyFrameReader bodyFrameReader = null;

        /// <summary>
        /// Array for the bodies
        /// </summary>
        private Body[] bodies = null;

        /// <summary>
        /// definition of bones
        /// </summary>
        private List<Tuple<JointType, JointType>> bones;

        /// <summary>
        /// Width of display (depth space)
        /// </summary>
        private int displayWidth;

        /// <summary>
        /// Height of display (depth space)
        /// </summary>
        private int displayHeight;

        /// <summary>
        /// List of colors for each body tracked
        /// </summary>
        private List<Pen> bodyColors;

        /// <summary>
        /// Current status text to display
        /// </summary>
        private string statusText = null;
        #endregion

        /// <summary>
        /// Initializes a new instance of the MainWindow class.
        /// </summary>
        public MainWindow()
        {
            // one sensor is currently supported
            this.kinectSensor = KinectSensor.GetDefault();

            // get the coordinate mapper
            this.coordinateMapper = this.kinectSensor.CoordinateMapper;

            // get the depth (display) extents
            FrameDescription frameDescription = this.kinectSensor.DepthFrameSource.FrameDescription;

            // get size of joint space
            this.displayWidth = frameDescription.Width;
            this.displayHeight = frameDescription.Height;

            // open the reader for the body frames
            this.bodyFrameReader = this.kinectSensor.BodyFrameSource.OpenReader();

            // a bone defined as a line between two joints
            this.bones = new List<Tuple<JointType, JointType>>();

            // Torso
            this.bones.Add(new Tuple<JointType, JointType>(JointType.Head, JointType.Neck));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.Neck, JointType.SpineShoulder));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineShoulder, JointType.SpineMid));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineMid, JointType.SpineBase));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineShoulder, JointType.ShoulderRight));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineShoulder, JointType.ShoulderLeft));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineBase, JointType.HipRight));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineBase, JointType.HipLeft));

            // Right Arm
            this.bones.Add(new Tuple<JointType, JointType>(JointType.ShoulderRight, JointType.ElbowRight));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.ElbowRight, JointType.WristRight));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.WristRight, JointType.HandRight));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.HandRight, JointType.HandTipRight));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.WristRight, JointType.ThumbRight));

            // Left Arm
            this.bones.Add(new Tuple<JointType, JointType>(JointType.ShoulderLeft, JointType.ElbowLeft));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.ElbowLeft, JointType.WristLeft));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.WristLeft, JointType.HandLeft));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.HandLeft, JointType.HandTipLeft));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.WristLeft, JointType.ThumbLeft));

            // Right Leg
            this.bones.Add(new Tuple<JointType, JointType>(JointType.HipRight, JointType.KneeRight));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.KneeRight, JointType.AnkleRight));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.AnkleRight, JointType.FootRight));

            // Left Leg
            this.bones.Add(new Tuple<JointType, JointType>(JointType.HipLeft, JointType.KneeLeft));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.KneeLeft, JointType.AnkleLeft));
            this.bones.Add(new Tuple<JointType, JointType>(JointType.AnkleLeft, JointType.FootLeft));

            // populate body colors, one for each BodyIndex
            this.bodyColors = new List<Pen>();

            this.bodyColors.Add(new Pen(Brushes.Red, 6));
            this.bodyColors.Add(new Pen(Brushes.Orange, 6));
            this.bodyColors.Add(new Pen(Brushes.Green, 6));
            this.bodyColors.Add(new Pen(Brushes.Blue, 6));
            this.bodyColors.Add(new Pen(Brushes.Indigo, 6));
            this.bodyColors.Add(new Pen(Brushes.Violet, 6));

            // set IsAvailableChanged event notifier
            this.kinectSensor.IsAvailableChanged += this.Sensor_IsAvailableChanged;

            // open the sensor
            this.kinectSensor.Open();

            // set the status text
            this.StatusText = this.kinectSensor.IsAvailable ? Properties.Resources.RunningStatusText
                                                            : Properties.Resources.NoSensorStatusText;

            // Create the drawing group we'll use for drawing
            this.drawingGroup = new DrawingGroup();

            // Create an image source that we can use in our image control
            this.imageSource = new DrawingImage(this.drawingGroup);

            // use the window object as the view model in this simple example
            this.DataContext = this;

            //初始化计算
            Init_inversKinematics();

            // initialize the components (controls) of the window
            this.InitializeComponent();

            //初始化通信方式
            Init_Communicate();

            //初始化舵机
            Init_servos();
        }

        /// <summary>
        /// INotifyPropertyChangedPropertyChanged event to allow window controls to bind to changeable data
        /// </summary>
        public event PropertyChangedEventHandler PropertyChanged;

        /// <summary>
        /// Gets the bitmap to display
        /// </summary>
        public ImageSource ImageSource
        {
            get
            {
                return this.imageSource;
            }
        }

        /// <summary>
        /// Gets or sets the current status text to display
        /// </summary>
        public string StatusText
        {
            get
            {
                return this.statusText;
            }

            set
            {
                if (this.statusText != value)
                {
                    this.statusText = value;

                    // notify any bound elements that the text has changed
                    if (this.PropertyChanged != null)
                    {
                        this.PropertyChanged(this, new PropertyChangedEventArgs("StatusText"));
                    }
                }
            }
        }

        /// <summary>
        /// Execute start up tasks
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void MainWindow_Loaded(object sender, RoutedEventArgs e)
        {
            if (this.bodyFrameReader != null)
            {
                this.bodyFrameReader.FrameArrived += this.Reader_FrameArrived;
            }
        }

        /// <summary>
        /// Execute shutdown tasks
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void MainWindow_Closing(object sender, CancelEventArgs e)
        {
            if (this.bodyFrameReader != null)
            {
                // BodyFrameReader is IDisposable
                this.bodyFrameReader.Dispose();
                this.bodyFrameReader = null;
            }

            if (this.kinectSensor != null)
            {
                this.kinectSensor.Close();
                this.kinectSensor = null;
            }

            if (tcpClient.TcpClient.Connected)
            {
                //tcpClient.TcpClient.fl
                //tcpClient.TcpClient.Close();
            }
        }

        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;
            DateTime start;
            DateTime end;
            start = System.DateTime.Now;
            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    //在后面的数据会存在链表中等待读取，需要手动地释放对象，手动置位

                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    //Console.WriteLine("body number : " + bodyFrame.BodyCount);
                    dataReceived = true;
                }

                end = System.DateTime.Now;
                //Console.WriteLine("acquare time : " + (end - start));
            }

            //数据处理部分
            if (dataReceived)
            {
                //using(BodyFrameReader bodyFrameReader = e.FrameReference.AcquireFrame().BodyFrameSource.OpenReader())
                {
                    //bodyFrameReader.IsPaused = true;

                    //遍历得到最近的人体
                    Body body_tracked = null;
                    float body_distance = 0xff;
                    foreach(Body body in bodies)
                    {
                        if (body.IsTracked)
                        {
                            if (body.Joints[JointType.SpineBase].Position.Z < body_distance)
                            {
                                //Console.WriteLine("getTrackedBody");
                                body_tracked = body;
                                body_distance = body.Joints[JointType.SpineBase].Position.Z;
                                //Console.WriteLine("body distance : " + body_distance);
                            }
                        }
                    }

                    if (body_tracked != null)
                    {
                        Joint rightHand = body_tracked.Joints[JointType.HandRight];
                        Joint rightShoulder = body_tracked.Joints[JointType.ShoulderRight];
                        Joint rightElbow = body_tracked.Joints[JointType.ElbowRight];

                        //目标点的相对坐标
                        float goal_x = rightHand.Position.X - rightShoulder.Position.X;
                        float goal_y = rightHand.Position.Y - rightShoulder.Position.Y;
                        float goal_z = rightHand.Position.Z - rightShoulder.Position.Z;
                        //float[] goal_array = { goal_x, goal_y, goal_z };
                        //调换顺序，并对Z取负值
                        float[] goal_array = { -goal_z, goal_x, goal_y };
                        //单位为m
                        //Console.WriteLine("goal: X:" + goal_x + "   Y:" + goal_y + "  Z:" + goal_z);


                        //壳的相对范围
                        float range_x = rightElbow.Position.X - rightShoulder.Position.X;
                        float range_y = rightElbow.Position.Y - rightShoulder.Position.Y;
                        float range_z = rightElbow.Position.Z - rightShoulder.Position.Z;
                        float[] range_array = { range_z, range_x, range_y };

                        //初始化向量
                        var vectorBuilder = MathNet.Numerics.LinearAlgebra.Vector<float>.Build;
                        var goalVector = vectorBuilder.DenseOfArray(goal_array);
                        var rangeVector = vectorBuilder.DenseOfArray(range_array);

                        //初始化矩阵
                        var matrixBuilder = MathNet.Numerics.LinearAlgebra.Single.Matrix.Build;
                        //var goalMatrix = matrixBuilder.DenseOfArray

                        //去除壳内的坐标，手的各坐标之和小于肘的坐标之和
                        double temp_goalLength = goal_x * goal_x + goal_y * goal_y + goal_z * goal_z;
                        double temp_rangeLength = range_x * range_x + range_y * range_y + range_z * range_z;
                        bool inHullFlag = false;
                        //Console.WriteLine("temp_goalLength : " + temp_goalLength);
                        //Console.WriteLine("temp_rangeLength : " + temp_rangeLength);
                        if (temp_goalLength > temp_rangeLength)
                        {
                            if (goal_y > 0)
                            {
                                inHullFlag = true;
                            }
                        }
                        if (inHullFlag)
                        {
                            //使用matlab计算角度
                            #region matlab

                            #endregion

                            #region InverserCalculate
                            System.Numerics.Complex[] thetas = roboArm.Calculate(goal_array);
                            Console.Write("theta:");
                            foreach(Complex complex in thetas)
                            {
                                Console.Write(" " + complex.ToString() + " ");
                            }
                            Console.WriteLine();
                            Console.WriteLine("goal: X:" + goal_x + "   Y:" + goal_y + "  Z:" + goal_z);
                            //svosld_servo1.sld_PWM.Value = Controls.Servo.deltaDegree2pwm(thetas[0], 120);
                            //svosld_servo2.sld_PWM.Value = Controls.Servo.deltaDegree2pwm(thetas[1], 120);

                            //svosld_servo3.sld_PWM.Value = Controls.Servo.deltaDegree2pwm(thetas[2], 270);
                            //svosld_servo4.sld_PWM.Value = Controls.Servo.deltaDegree2pwm(thetas[3], 270);
                            //svosld_servo5.sld_PWM.Value = Controls.Servo.deltaDegree2pwm(thetas[4], 270);

                            //svosld_servo6.sld_PWM.Value = Controls.Servo.deltaDegree2pwm(thetas[5], 180);

                            //依据数值对角度做正负号上的一些修正？
                            //只是尝试
                            //只取实数部分？或者是取相位？
                            setDegree2deltaPwm(thetas[0].Real, 1);
                            setDegree2deltaPwm(thetas[1].Real, 2);
                            setDegree2deltaPwm(thetas[2].Real, 3);
                            setDegree2deltaPwm(thetas[3].Real, 4);
                            setDegree2deltaPwm(thetas[4].Real, 5);
                            setDegree2deltaPwm(thetas[5].Real, 6);
                            //setDegree2deltaPwm(thetas[0], 1);
                            //System.Threading.Thread.Sleep(1000);
                            #endregion

                        }
                        if (body_tracked.HandRightState == HandState.Closed)
                        {
                            setHand(2000);

                        }
                        else if (body_tracked.HandRightState == HandState.Open)
                        {
                            setHand(1300);
                        }
                    }
                    //停顿一秒
                    //System.Threading.Thread.Sleep(1000);
                    end = System.DateTime.Now;
                    //Console.WriteLine("process time : " + (end - start));
                }
            }

            //画图部分
            if (dataReceived)
            {
                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    // Draw a transparent background to set the render size
                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                    int penIndex = 0;
                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary<JointType, Joint> joints = body.Joints;

                            // convert the joint points to depth (display) space
                            Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                CameraSpacePoint position = joints[jointType].Position;
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                            }

                            this.DrawBody(joints, jointPoints, dc, drawPen);

                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                        }
                    }

                    // prevent drawing outside of our render area
                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                }
                end = System.DateTime.Now;
                //Console.WriteLine("draw time : " + (end - start));
            }
        }

        #region Draw_Function
        /// <summary>
        /// Draws a body
        /// </summary>
        /// <param name="joints">joints to draw</param>
        /// <param name="jointPoints">translated positions of joints to draw</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        /// <param name="drawingPen">specifies color to draw a specific body</param>
        private void DrawBody(IReadOnlyDictionary<JointType, Joint> joints, IDictionary<JointType, Point> jointPoints, DrawingContext drawingContext, Pen drawingPen)
        {
            // Draw the bones
            foreach (var bone in this.bones)
            {
                this.DrawBone(joints, jointPoints, bone.Item1, bone.Item2, drawingContext, drawingPen);
            }

            // Draw the joints
            foreach (JointType jointType in joints.Keys)
            {
                Brush drawBrush = null;

                TrackingState trackingState = joints[jointType].TrackingState;

                if (trackingState == TrackingState.Tracked)
                {
                    drawBrush = this.trackedJointBrush;
                }
                else if (trackingState == TrackingState.Inferred)
                {
                    drawBrush = this.inferredJointBrush;
                }

                if (drawBrush != null)
                {
                    drawingContext.DrawEllipse(drawBrush, null, jointPoints[jointType], JointThickness, JointThickness);
                }
            }
        }

        /// <summary>
        /// Draws one bone of a body (joint to joint)
        /// </summary>
        /// <param name="joints">joints to draw</param>
        /// <param name="jointPoints">translated positions of joints to draw</param>
        /// <param name="jointType0">first joint of bone to draw</param>
        /// <param name="jointType1">second joint of bone to draw</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        /// /// <param name="drawingPen">specifies color to draw a specific bone</param>
        private void DrawBone(IReadOnlyDictionary<JointType, Joint> joints, IDictionary<JointType, Point> jointPoints, JointType jointType0, JointType jointType1, DrawingContext drawingContext, Pen drawingPen)
        {
            Joint joint0 = joints[jointType0];
            Joint joint1 = joints[jointType1];

            // If we can't find either of these joints, exit
            if (joint0.TrackingState == TrackingState.NotTracked ||
                joint1.TrackingState == TrackingState.NotTracked)
            {
                return;
            }

            // We assume all drawn bones are inferred unless BOTH joints are tracked
            Pen drawPen = this.inferredBonePen;
            if ((joint0.TrackingState == TrackingState.Tracked) && (joint1.TrackingState == TrackingState.Tracked))
            {
                drawPen = drawingPen;
            }

            drawingContext.DrawLine(drawPen, jointPoints[jointType0], jointPoints[jointType1]);
        }

        /// <summary>
        /// Draws a hand symbol if the hand is tracked: red circle = closed, green circle = opened; blue circle = lasso
        /// </summary>
        /// <param name="handState">state of the hand</param>
        /// <param name="handPosition">position of the hand</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        private void DrawHand(HandState handState, Point handPosition, DrawingContext drawingContext)
        {
            switch (handState)
            {
                case HandState.Closed:
                    drawingContext.DrawEllipse(this.handClosedBrush, null, handPosition, HandSize, HandSize);
                    break;

                case HandState.Open:
                    drawingContext.DrawEllipse(this.handOpenBrush, null, handPosition, HandSize, HandSize);
                    break;

                case HandState.Lasso:
                    drawingContext.DrawEllipse(this.handLassoBrush, null, handPosition, HandSize, HandSize);
                    break;
            }
        }

        /// <summary>
        /// Draws indicators to show which edges are clipping body data
        /// </summary>
        /// <param name="body">body to draw clipping information for</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        private void DrawClippedEdges(Body body, DrawingContext drawingContext)
        {
            FrameEdges clippedEdges = body.ClippedEdges;

            if (clippedEdges.HasFlag(FrameEdges.Bottom))
            {
                drawingContext.DrawRectangle(
                    Brushes.Red,
                    null,
                    new Rect(0, this.displayHeight - ClipBoundsThickness, this.displayWidth, ClipBoundsThickness));
            }

            if (clippedEdges.HasFlag(FrameEdges.Top))
            {
                drawingContext.DrawRectangle(
                    Brushes.Red,
                    null,
                    new Rect(0, 0, this.displayWidth, ClipBoundsThickness));
            }

            if (clippedEdges.HasFlag(FrameEdges.Left))
            {
                drawingContext.DrawRectangle(
                    Brushes.Red,
                    null,
                    new Rect(0, 0, ClipBoundsThickness, this.displayHeight));
            }

            if (clippedEdges.HasFlag(FrameEdges.Right))
            {
                drawingContext.DrawRectangle(
                    Brushes.Red,
                    null,
                    new Rect(this.displayWidth - ClipBoundsThickness, 0, ClipBoundsThickness, this.displayHeight));
            }
        }
        #endregion

        /// <summary>
        /// Handles the event which the sensor becomes unavailable (E.g. paused, closed, unplugged).
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Sensor_IsAvailableChanged(object sender, IsAvailableChangedEventArgs e)
        {
            // on failure, set the status text
            this.StatusText = this.kinectSensor.IsAvailable ? Properties.Resources.RunningStatusText
                                                            : Properties.Resources.SensorNotAvailableStatusText;
        }


    }
}
